/* Copyright 2017 The Apollo Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "modules/map/hdmap/adapter/odr_adapter.h"

#include <vector>

#include "modules/map/hdmap/adapter/proto_organizer.h"
#include "modules/map/hdmap/adapter/xml_parser/status.h"

#include "modules/map/hdmap/adapter/odr_parser/convert_to_hdmap.h"
#include "modules/map/hdmap/adapter/odr_parser/util_odr_parser.h"

#include "modules/map/hdmap/adapter/odr_parser/roadengine_api.h"

namespace apollo {
namespace hdmap {
namespace adapter {

bool OdrAdapter::LoadData(const std::string& filename, apollo::hdmap::Map* pb_map) {

  // 检查是否加密的文件并且解析
  std::string decode_string;
  int result = check_and_decode(filename, decode_string);
  if (result >= 0) {
    ADEBUG << "check_and_decode success";
  } else {
    ADEBUG << "check_and_decode failed";
  }

  tinyxml2::XMLDocument document;

  if (document.Parse(decode_string.c_str(), result) != tinyxml2::XML_SUCCESS) {
      AERROR << "OdrAdapter fail to load file " << decode_string;
      return false;
  }

  OdrMap map_;

  bool reverseXAxis = false;

  // root node
  const tinyxml2::XMLElement* root_node = document.RootElement();
  CHECK(root_node != nullptr);
  // header
  PbHeader* map_header = pb_map->mutable_header();
  Status status = HeaderOdrParser::Parse(*root_node, map_header, reverseXAxis);
  if (!status.ok()) {
    AERROR << "OdrAdapter fail to parse opendrive header, " << status.error_message();
    //return false;
  }

  // roads
  //std::vector<RoadInternal> roads;
  status = RoadsOdrParser::Parse(*root_node, map_.roads);
  if (!status.ok()) {
    AERROR << "OdrAdapter fail to parse opendrive road, " << status.error_message();
    return false;
  }

  // junction
  //std::vector<JunctionInternal> junctions;
  status = JunctionsOdrParser::Parse(*root_node, map_.junctions);
  if (!status.ok()) {
    AERROR << "OdrAdapter fail to parse opendrive junction, " << status.error_message();
    return false;
  }

  // Fill lane linkage
  FilleLaneLinkage(map_);

  std::map<std::int64_t, rg::RoadGeometry::Ptr> roadGeometries;
  if (!roadengine::GetRoadData(decode_string, reverseXAxis, roadGeometries))
  {
    AERROR << "roadengine fails to parse opendrive to get road geometries " << status.error_message();
    return false;
  }
  
  // convert XML data to HDMap
  //
  std::vector<RoadInternal> roads;
  OdrToHdmapConverter::ConvertRoads(map_, roads, roadGeometries, reverseXAxis);
  //OdrToHdmapConverter::ConvertLaneLink(map_, roads);
  //OdrToHdmapConverter

  std::vector<JunctionInternal> junctions;
  OdrToHdmapConverter::ConvertJunctions(map_, roads, junctions);

  ProtoOrganizer proto_organizer;
  proto_organizer.GetRoadElements(&roads);
  proto_organizer.GetJunctionElements(junctions);
  proto_organizer.GetOverlapElements(roads, junctions);
  proto_organizer.OutputData(pb_map);

  //
  // for(int i=0;i<pb_map->road_size();i++   ){
  //     // road
  //     pb_map->road(i);
  //     // road_section
  //     for(int j=0;j<pb_map->road(i).section_size();j++){
  //         pb_map->road(i).section(j);
  //          // lane
  //         for(int k=0;k<pb_map->road(i).section(j).lane_id_size();k++){
  //           std::cout<<"  Lane ID  "<<pb_map->road(i).section(j).lane_id(k).id()
  //                    <<"  i  "<<i<<"  j  "<<j<<"  k  "<<k<<std::endl;
  //         }
  //     }
  //   }
  
  return true;
}











namespace {
bool IsStartSection(LaneSection* laneSection)
{
  // s is zero
  return laneSection->s < 0.000001;
}

bool IsEndSection(LaneSection* laneSection)
{
  // e is -1
  return laneSection->e < -0.999;
}
}



// 检查是否加密的文件并且解析
int OdrAdapter::check_and_decode(const std::string& infile, std::string& out_string) {
  typedef std::shared_ptr<void> defer_ptr;

  std::ifstream in(infile, std::ios::binary);
  if (!in) return -1;
  defer_ptr __close_in__(nullptr, [&](...) { in.close(); });

  std::string tag("51vr-od-v0001");
  char buf[20] = {};
  in.read(buf, tag.length());
  if (!in) return -1;

  // 无需解密，直接读文件
  if (tag != std::string(buf)) {
  std::string odraw;
    char pair[1];
    while (in.peek() != EOF){
      in.read(pair, 1);  
      odraw.append(&pair[0], 1);
      } 
    out_string = odraw;
    return int(odraw.length());
  }
  
  std::string raw;
  char pair[2];
  while (in.peek() != EOF){
    in.read(pair, 2);
    char temp= char(  ((std::uint8_t)pair[0] * (std::uint8_t)pair[0]) % 256  );
    temp= (char) (temp);
    pair[1] = (char)(pair[1] + temp);
    pair[1] = (char) (pair[1]);
    raw.append(&pair[1], 1);
  } 
  out_string = raw;
  return int(raw.length());
}


// process lane link element to setup link between lanes
void  OdrAdapter::FilleLaneLinkage(OdrMap& map)
{
  auto roadIt = map.roads.begin();
  for (; roadIt != map.roads.end(); ++roadIt) {
    //process roads outside junction first
    Road* road = roadIt->second;

    if (road && road->junction_id != "-1")
      continue;

    Predecessor* predecessor = road->link.predecessor;
    Predecessor* successor = road->link.successor;

    auto checkLinkedRoadSameDir = []
    (const Road* road1, const Road* road2, bool preRoad)->bool
    {
      // This method check if road1's direction is opposite to road2 or not.
      // There are two types of road link topology:
      // CASE 1: -->-->--> ROAD1 -->-->--> (LINK) -->-->--> ROAD2 -->-->-->
      // Return true
      // CASE 2: -->-->--> ROAD1 -->-->--> (LINK) <--<--<-- ROAD2 <--<--<--
      // Return false
      Predecessor* link1 = preRoad ?
        road1->link.predecessor : road1->link.successor;
      if (!link1)
      {
        // The road to test do not have valid links. 
        AERROR << "Cannot find valid Road link in Road"
          << road1->id;
        return true;
      }

      // TODO: We should only consider contactpoint between roads,
      // but there is a blocker should be fixed first.
      // JIRA: http://jira.51hitech.com/browse/WORLD4AD-1935
      Predecessor* link2 = preRoad ?
        road2->link.successor : road2->link.predecessor;
      if (!link2)
      {
        // The road to test do not have valid links. 
        AERROR << "Cannot find valid Road link in Road"
          << road2->id;
        return true;
      }

      if ((link1->elementId == road2->id) && (link2->elementId == road1->id))
      {
        return true; // Case 1 
      }
      else
      {
        return false; // Case 2
      }

    };

    const std::string& preId = predecessor ? predecessor->elementId : "";
    const std::string& sucId = successor ? successor->elementId : "";
#if 1
    if (predecessor && predecessor->elementType == ROAD)
    {
      Road* preRoad = UtilOdrParser::FindRoad(map.roads, preId);
      if (preRoad->junction_id != "-1")
      {
        AERROR << "Wrong predecessor information for road " << road->id << ": elementType is road but it is inside junction, predecessor elementId " << preRoad->id;
        continue;
      }

    }
    if (successor && successor->elementType == ROAD)
    {
      Road* sucRoad = UtilOdrParser::FindRoad(map.roads, sucId);
      if (sucRoad->junction_id != "-1")
      {
        AERROR << "Wrong successor information for road " << road->id << ": elementType is road but it is inside junction, successor elementId " << sucRoad->id;
        continue;
      }
    }
#endif
    Road* preRoad = predecessor && predecessor->elementType == ROAD ? UtilOdrParser::FindRoad(map.roads, preId) : NULL;
    Road* sucRoad = successor && successor->elementType == ROAD ? UtilOdrParser::FindRoad(map.roads, sucId) : NULL;

    bool preRoadSameDir = preRoad && checkLinkedRoadSameDir(road, preRoad, true/*preRoad*/);
    bool sucRoadSameDir = sucRoad && checkLinkedRoadSameDir(road, sucRoad, false/*sucRoad*/);

    bool inner_pre_lane;
    bool inner_suc_lane;
    std::string connect_lane_id;
    auto section_num = road->laneSections.size();
    for (size_t si = 0; si < section_num; si++) {
      LaneSection* laneSection = road->laneSections[si];

      // For right-side lanes, the lane link defination in hdmap same as Opendrive defines,
      // only need to consider is Opendrive's Current Road's direction is same as the Next Road or not.
      // There are two cases:
      // CASE 1: -->-->--> Cur Road -->-->--> (LINK) -->-->--> Next Road -->-->-->
      // CASE 2: -->-->--> Cur Road -->-->--> (LINK) <--<--<-- Next Road <--<--<--
      // For Case 1, need to link the Current Road's end section to the Next road's start section,
      // For Case 2, need to link the Current Road's end section to the Next road's end section.
      for (size_t i = 0; i < laneSection->right.size(); i++) {
        Lane* lane = laneSection->right[i];
        // only process driving lane
        if (lane->type != DRIVING) {
          continue;
        }

        Link& link = lane->link;
        inner_pre_lane = true;     // previous lane is in same road, different lane section by default
        inner_suc_lane = true;     // successor lane is in same road, different lane section by default
        if (IsEndSection(laneSection)) {
          inner_suc_lane = false;
          if (link.successor && sucRoad) {
            Lane* sucLane = FindLaneInRoad(sucRoad, link.successor->elementId,
              sucRoadSameDir/*StartSection by default*/);
            if (sucLane)
            {
              AddLaneLink(sucLane, sucRoad->id, lane, road->id);
            }
            else
            {
              AERROR << "Get successor link for lane " << lane->uid << ": successor road " << sucRoad->id << " missing lane " << link.successor->elementId;
            }
          }
        }
        if (IsStartSection(laneSection)) {
          inner_pre_lane = false;
          if (link.predecessor && preRoad) {
            Lane* preLane = FindLaneInRoad(preRoad, link.predecessor->elementId,
              !preRoadSameDir/*EndSection by default*/);
            if (preLane)
            {
              AddLaneLink(lane, road->id, preLane, preRoad->id);
            }
            else
            {
              AERROR << "Get predecessor link for lane " << lane->uid << ": predecessor road " << preRoad->id << " missing lane " << link.predecessor->elementId;
            }
          }
        }

        // Then link the internal section one by one, logic direction(predecessor/successor)
        // defined same as Opendrive spec. 
        if (inner_pre_lane) {
          connect_lane_id = link.predecessor ? link.predecessor->elementId : lane->id;
          Lane* preLane = FindLaneInLaneSection(road->laneSections[si - 1], connect_lane_id);
          if (preLane) {

              AddLaneLink(lane, road->id, preLane, road->id);
            // if (preLane->link.successor == NULL || preLane->link.successor->elementId == lane->id) {
            //   AddLaneLink(lane, road->id, preLane, road->id);
            // }
            // else
            // {
            //   AERROR << "Get predecessor link for lane " << lane->uid << " failed, conflict its preLane "<< preLane->uid<<" 's successor is not right!";
            // }


          }
          else
          {
            AERROR << "Get predecessor link for lane " << lane->uid <<" failed, in previous lane section can not find id"<< connect_lane_id;
          }
        }
        if (inner_suc_lane) {
          connect_lane_id = link.successor ? link.successor->elementId : lane->id;
          Lane* sucLane = FindLaneInLaneSection(road->laneSections[si + 1], connect_lane_id);
          if (sucLane) {

              AddLaneLink(sucLane, road->id, lane, road->id);
            // if (sucLane->link.predecessor == NULL || sucLane->link.predecessor->elementId == lane->id) {
            //   AddLaneLink(sucLane, road->id, lane, road->id);
            // }
            // else
            // {
            //   AERROR << "Get successor link for lane " << lane->uid << " failed, conflict its sucLane " << sucLane->uid << " 's predecessor is not right!";
            // }


          }
          else
          {
            AERROR << "Get successor link for lane " << lane->uid << " failed, in next lane section can not find id" << connect_lane_id;
          }
        }

      }

      // For left-side lanes, the lane link defination in hdmap opposite to Opendrive defines,
      for (size_t i = 0; i < laneSection->left.size(); i++) {
        Lane* lane = laneSection->left[i];

        // only process driving lane
        if (lane->type != DRIVING) {
          continue;
        }

        Link& link = lane->link;

        // CASE 1: -->-->--> Cur Road -->-->--> (LINK) -->-->--> Next Road -->-->-->
        // CASE 2: -->-->--> Cur Road -->-->--> (LINK) <--<--<-- Next Road <--<--<--
        // For Case 1, need to link the Next Road's end section to the Current road's start section,
        // For Case 2, need to link the Next Road's end section to the Current road's end section.
        inner_pre_lane = true;     // previous lane is in same road, different lane section by default
        inner_suc_lane = true;     // successor lane is in same road, different lane section by default
        if (IsStartSection(laneSection)) {
          // this section should be considered as the last connection to the predecessor
          inner_pre_lane = false;
          if (link.predecessor && preRoad) {
            Lane* preLane = FindLaneInRoad(preRoad, link.predecessor->elementId,
              !preRoadSameDir/*endSection by default*/);
            if (preLane)
            {
              AddLaneLink(preLane, preRoad->id, lane, road->id);
            }
            else
            {
              AERROR << "Get predecessor link for lane " << lane->uid << ": predecessor road " << preRoad->id << " missing lane " << link.predecessor->elementId;
            }
          }
        }
        if (IsEndSection(laneSection)) {
          // this section should be considered as the first connection from the successor
          inner_suc_lane = false;
          if (link.successor && sucRoad) {
            Lane* sucLane = FindLaneInRoad(sucRoad, link.successor->elementId,
              sucRoadSameDir);
            if (sucLane)
            {
              AddLaneLink(lane, road->id, sucLane, sucRoad->id);
            }
            else
            {
              AERROR << "Get successor link for lane " << lane->uid << ": successor road " << sucRoad->id << " missing lane " << link.successor->elementId;
            }
          }
        }

        if (inner_pre_lane) {
          connect_lane_id = link.predecessor ? link.predecessor->elementId : lane->id;
          Lane* preLane = FindLaneInLaneSection(road->laneSections[si - 1], connect_lane_id);
          if (preLane) {

              AddLaneLink(preLane, road->id, lane, road->id);
            // if (preLane->link.successor == NULL || preLane->link.successor->elementId == lane->id) {
            //   AddLaneLink(preLane, road->id, lane, road->id);
            // }
            // else
            // {
            //   AERROR << "Get predecessor link for lane " << lane->uid << " failed, conflict its preLane " << preLane->uid << " 's successor is not right!";
            // }


          }
          else
          {
            AERROR << "Get predecessor link for lane " << lane->uid << " failed, in previous lane section can not find id" << connect_lane_id;
          }
        }
        if (inner_suc_lane) {
          connect_lane_id = link.successor ? link.successor->elementId : lane->id;
          Lane* sucLane = FindLaneInLaneSection(road->laneSections[si + 1], connect_lane_id);
          if (sucLane) {

              AddLaneLink(lane, road->id, sucLane, road->id);
            // if (sucLane->link.predecessor == NULL || sucLane->link.predecessor->elementId == lane->id) {
            //   AddLaneLink(lane, road->id, sucLane, road->id);
            // }
            // else
            // {
            //   AERROR << "Get successor link for lane " << lane->uid << " failed, conflict its sucLane " << sucLane->uid << " 's predecessor is not right!";
            // }


          }
          else
          {
            AERROR << "Get successor link for lane " << lane->uid << " failed, in next lane section can not find id" << connect_lane_id;
          }
        }
      }
    }

  }

  // process junction
  // most junction connection is duplicated with lane link
  // some connection is invalid, how are they generated? sumo netconvert bug? 
  //            or original sumo net map is inaccurate?
  //
  auto junctionIt = map.junctions.begin();
  for (; junctionIt != map.junctions.end(); ++junctionIt) {
    Junction* junc = junctionIt->second;

    for (size_t i = 0; i < junc->connections.size(); i++) {
      Connection* connection = junc->connections[i];

      const std::string& incoming = connection->incomingRoad;
      const std::string& connecting = connection->connectingRoad;

      Road* internal = UtilOdrParser::FindRoad(map.roads, connecting);
      if (!internal)
      {
        AERROR <<"For junction " << junc->id<< ": Cannot find connection road " << connecting ;
        continue;
      }
      if (internal->junction_id != junc->id)
      {
        AERROR << "For junction " << junc->id<< ": Bad connection road declared "<< connecting<<", it's not inside this junction";
        continue;
      }

      for (size_t lli = 0; lli < connection->laneLinkFroms.size(); lli++) {
        const std::string& fromLane = connection->laneLinkFroms[lli];
        const std::string& toLane = connection->laneLinkTos[lli];
        ProcessLaneLink(map.roads, incoming, fromLane, connecting, toLane, junc->id,
          connection->contactPoint == START);
      } // for lli
    } // for i
  } // for junctionIt
}



void OdrAdapter::ProcessLaneLink(const std::map<std::string, Road*>& roads,
                                 const std::string& fromRoad,
                                 const std::string& fromLane,
                                 const std::string& toRoad,
                                 const std::string& toLane,
                                 const std::string& junctionId,
                                 bool contactStartPoint)
{
  Road* from = UtilOdrParser::FindRoad(roads, fromRoad);
  Road* internal = UtilOdrParser::FindRoad(roads, toRoad);

  if (!from || !internal) {
    AERROR << "For junction " << junctionId <<": Cannot find FROM road for connection between "
           << from->id << " and " << internal->id;
    return;
  }
  if (!internal->link.successor)
  {
    AERROR << "For junction " << junctionId << ": Cannot find successor road for "<<internal->id;
    return;
  }
  Road* to = UtilOdrParser::FindRoad(
    roads, internal->link.successor->elementId);

  if (!to) {
    AERROR << "For junction " << junctionId << ": Cannot find TO road for connection between "
           << from->id << " and " << internal->id;
    return;
  }

  if (!internal->link.predecessor || !internal->link.successor) 
  {
    AERROR << "For junction " << junctionId << ": Cannot find link information for connection between "
           << from->id << " and " << to->id;
    return;
  }

  Lane* fLane = 
    FindLaneInRoad(from, fromLane, internal->link.predecessor->contactPoint == START);
  // FIXME: For internal lane, only contactStartPoint == START is supported right now.
  Lane* iLane = 
    FindLaneInRoad(internal, toLane, contactStartPoint);

  if (!fLane || !iLane) {
    AERROR << "For junction " << junctionId << ": Cannot find lane for laneLink between "
           << from->id << " and " << internal->id;
    return;
  }

  if (!iLane->link.predecessor || !iLane->link.successor) 
  {
    AERROR << "For junction " << junctionId << ": Cannot find lane link information in internal road "
           << internal->id << " lane " << toLane;
    return;
  }
  Lane* tLane = 
    FindLaneInRoad(to, 
      iLane->link.successor->elementId, internal->link.successor->contactPoint == START);

  if (!tLane) {
    AERROR << "For junction " << junctionId << ": Cannot find lane for laneLink between "
           << internal->id << " and " << to->id;
    return;
  }

  AddLaneLink(iLane, internal->id, fLane, from->id);
  AddLaneLink(tLane, to->id, iLane, internal->id);
}

Lane* OdrAdapter::FindLaneInRoad(Road* road, const std::string& laneIdStr, bool startSection)
{
  Lane* fLane = NULL;

  int laneId = std::stoi(laneIdStr);
  if (laneId < 0) {
    // right
    for (size_t i = 0; i < road->laneSections.size(); i++) {
      if ((startSection && IsStartSection(road->laneSections[i])) ||
          (!startSection && IsEndSection(road->laneSections[i])) ) {
        std::vector<Lane*>& right = road->laneSections[i]->right;
        if (!right.empty()) {
          for (size_t j = 0; j < right.size(); j++) {
            if (right[j]->id == laneIdStr) {
              fLane = right[j];
              break;
            }
          }
          break;
        }
      }
    }
  } else if (laneId > 0) {
    // left
    for (size_t i = 0; i < road->laneSections.size(); i++) {
      if ((startSection && IsStartSection(road->laneSections[i])) ||
          (!startSection && IsEndSection(road->laneSections[i])) ) {
        std::vector<Lane*>& left = road->laneSections[i]->left;
        if (!left.empty()) {
          for (size_t j = 0; j < left.size(); j++) {
            if (left[j]->id == laneIdStr) {
              fLane = left[j];
              break;
            }
          }
          break;
        }
      }
    }

  } else {
    ; // cannot be center
  }

  return fLane;
}

Lane* OdrAdapter::FindLaneInLaneSection(const LaneSection* section, const std::string& lane_id)
{
  Lane* lane = NULL;
  if (section) {
    int id = std::atoi(lane_id.c_str());
    if (id == 0) {
        lane = section->center;
    } else if (id < 0) {
      for (size_t i = 0; i < section->right.size(); i++) {
        if (section->right[i]->id == lane_id) {
          lane = section->right[i];
          break;
        }
      }
    } else {
      for (size_t i = 0; i < section->left.size(); i++) {
        if (section->left[i]->id == lane_id) {
          lane = section->left[i];
          break;
        }
      }
    }
    
  }

  return lane;
}

void OdrAdapter::AddLaneLink(Lane* lane, const std::string& roadId,
                             Lane* preLane, const std::string& preRoadId)
{
        TmpLaneConnection tmpCon;
        tmpCon.roadId = roadId;
        tmpCon.laneId = lane->id;
        tmpCon.laneUid = lane->uid;
        // check duplicate link
        bool found = false;
        for (size_t i = 0; i < preLane->successors.size(); i++) {
          TmpLaneConnection& con = preLane->successors[i];
          if (con.roadId == tmpCon.roadId && con.laneUid == tmpCon.laneUid) {
            found  = true;
            break;
          }
        }
        if (!found) {
          preLane->successors.push_back(tmpCon);
        }

        tmpCon.roadId = preRoadId;
        tmpCon.laneId = preLane->id;
        tmpCon.laneUid = preLane->uid;
        // check duplicate link
        found = false;
        for (size_t i = 0; i < preLane->predecessors.size(); i++) {
          TmpLaneConnection& con = preLane->predecessors[i];
          if (con.roadId == tmpCon.roadId && con.laneUid == tmpCon.laneUid) {
            found  = true;
            break;
          }
        }
        if (!found) {
          lane->predecessors.push_back(tmpCon);
        }
}

}  // namespace adapter
}  // namespace hdmap
}  // namespace apollo
